#include "delay.h"

#define DEMCR (*(unsigned int *)0xE000EDFC)
#define TRCENA (0x01 << 24)
#define DWT_CTRL (*(unsigned int *)0xE0001000)
#define CYCCNTENA (0x01 << 0)
#define DWT_CYCCNT (*(volatile unsigned int *)0xE0001004)

int Clock_Freq;

void delay_init(int sys_clk) {
  DEMCR |= TRCENA;
  DWT_CTRL |= CYCCNTENA;
  Clock_Freq = sys_clk * 1000000;
}

void delay_us(int uSec) {
  int ticks_start = DWT_CYCCNT,
      ticks_delay = (uSec * (Clock_Freq / (1000 * 1000))),
      ticks_end = ticks_start + ticks_delay;

  if (ticks_end > ticks_start) {
    while (DWT_CYCCNT < ticks_end)
      ;
  } else {
    while (DWT_CYCCNT >= ticks_end)
      ;
    while (DWT_CYCCNT < ticks_end)
      ;
  }
}
